using System;
using System.Collections.Generic;
using System.Text;
using System.Drawing;
using System.Drawing.Imaging;

namespace Blimpbots.Recognition
{
    public class GrayscaleImage
    {
        public double[] Scan0;
        private int _width;
        private int _height;
        public int Width { get { return _width; } }
        public int Height { get { return _height; } }

        /// <summary>
        /// Creates a 0.0 to 1.0 grayscale image from a bitmap.
        /// </summary>
        public GrayscaleImage(Bitmap bitmap, Rectangle region)
        {
            Convert(bitmap, region);
        }

        /// <summary>
        /// Creates a 0.0 to 1.0 grayscale image from a bitmap.
        /// </summary>
        public GrayscaleImage(Bitmap bitmap)
        {
            Convert(bitmap, new Rectangle(0,0, bitmap.Width, bitmap.Height));
        }

        public GrayscaleImage(Bitmap bitmap, Color peak)
        {
            Convert2(bitmap, new Rectangle(0, 0, bitmap.Width, bitmap.Height), peak);
        }

        private void Convert2(Bitmap bitmap, Rectangle region, Color peak)
        {
            Color two = Color.FromArgb(152, 186, 188);

            double h, s, v;
            //RGBtoHSV(peak.R / 255.0, peak.G / 255.0, peak.B / 255.0, out h, out s, out v);

            BitmapData bmData = bitmap.LockBits(new Rectangle(0, 0, bitmap.Width, bitmap.Height),
            System.Drawing.Imaging.ImageLockMode.ReadWrite,
            System.Drawing.Imaging.PixelFormat.Format32bppArgb);

            _width = region.Width;
            _height = region.Height;
            int offset_top = region.Top * bitmap.Width * 4;
            int offset_left = region.Left * 4;
            int offset_right = (bitmap.Width - region.Right) * 4;

            Scan0 = new double[_width * _height];

            unsafe
            {
                int i = 0;

                byte* ptrBitmap = (byte*)bmData.Scan0;

                ptrBitmap += offset_top;

                for (int y = 0; y < _height; y++)
                {
                    ptrBitmap += offset_left;
                    for (int x = 0; x < _width; x++)
                    {
                        RGBtoHSV(ptrBitmap[0] / 255.0, ptrBitmap[1] / 255.0, ptrBitmap[2] / 255.0, out h, out s, out v); 

                        //Scan0[i] = (ptrBitmap[0] * 0.3 +
                        //            ptrBitmap[1] * 0.59 + ptrBitmap[2] * 0.11) / 255.0;

                        Scan0[i] = 0.5;

                        //ptrBitmap[0] = (byte)(Math.Abs(h - peakH) * 0.707);
                        //ptrBitmap[1] = (byte)(Math.Abs(h - peakH) * 0.707);
                        //ptrBitmap[2] = (byte)(Math.Abs(h - peakH) * 0.707);

                        int dist = (int)( Math.Sqrt( Math.Pow((double)(peak.R - ptrBitmap[0]), 2)
                                  + Math.Pow((double)(peak.G - ptrBitmap[1]), 2)
                                  + Math.Pow((double)(peak.B - ptrBitmap[2]), 2)
                                  + Math.Pow((double)(two.R - ptrBitmap[0]), 2)
                                  + Math.Pow((double)(two.G - ptrBitmap[2]), 2)
                                  + Math.Pow((double)(two.B - ptrBitmap[2]), 2)));


                        if (dist > 255) dist = 255;

                        ptrBitmap[0] = (byte)(255-dist);
                        ptrBitmap[1] = (byte)(255-dist);
                        ptrBitmap[2] = (byte)(255-dist);

                        ptrBitmap += 4; // advance to the next pixel
                        i++;            // "
                    }
                    ptrBitmap += offset_right;
                }
            }

            bitmap.UnlockBits(bmData);
        }

        void RGBtoHSV(double r, double g, double b, 
                      out double h, out double s, out double v)
        {
            double min = (r < g) ? (r < b) ? r : b : (g < b) ? g : b;
            double max = (r > g) ? (r > b) ? r : b : (g > b) ? g : b;
            v = max;				// v
            double delta = max - min;
            if (max != 0)
                s = delta / max;    // s
            else
            {
                // r = g = b = 0	// s = 0, v is undefined
                s = 0;
                h = -1;
                v = 0;
                return;
            }
            if (r == max)
                h = (g - b) / delta;		// between yellow & magenta
            else if (g == max)
                h = 2 + (b - r) / delta;	// between cyan & yellow
            else
                h = 4 + (r - g) / delta;	// between magenta & cyan

            h *= 60;				        // degrees
            if (h < 0)
                h += 360;
        }



        private void Convert(Bitmap bitmap, Rectangle region)
        {

            BitmapData bmData = bitmap.LockBits(new Rectangle(0, 0, bitmap.Width, bitmap.Height),
            System.Drawing.Imaging.ImageLockMode.ReadOnly,
            System.Drawing.Imaging.PixelFormat.Format32bppArgb);

            _width = region.Width;
            _height = region.Height;
            int offset_top = region.Top * bitmap.Width * 4;
            int offset_left = region.Left * 4;
            int offset_right = (bitmap.Width - region.Right) * 4;

            Scan0 = new double[_width * _height];

            unsafe
            {
                int i = 0;

                byte* ptrBitmap = (byte*)bmData.Scan0;

                ptrBitmap += offset_top;

                for (int y = 0; y < _height; y++)
                {
                    ptrBitmap += offset_left;
                    for (int x = 0; x < _width; x++)
                    {
                        Scan0[i] = (ptrBitmap[0] * 0.3 +
                                    ptrBitmap[1] * 0.59 + ptrBitmap[2] * 0.11) / 255.0;
                        ptrBitmap += 4; // advance to the next pixel
                        i++;            // "
                    }
                    ptrBitmap += offset_right;
                }
            }

            bitmap.UnlockBits(bmData);
        }
    }
}
